/**
 * 
 */
package robocup.component.worldmodel.particlefilter;

import java.util.Random;

import robocup.component.geometry.Vektor;
import robocup.component.infotypes.BallInfo;

/**
 * @author werwurm
 * 
 */
public class BallSpeedRegion {

  private final static double PI_180       = Math.PI / 180.0;

  private final static double _180_PI      = 180.0 / Math.PI;

  private double        minDeltaR;

  private double        maxDeltaR;

  private double        minDeltaPhi;

  private double        maxDeltaPhi;

  private double        seenDistance;

  private double        ex, ey;

  private double        headangle;

  private Vektor        mySpeed;

  private final Vektor        helperVektor = new Vektor();

  private final Random        random       = new Random(System.currentTimeMillis());

  /**
   * A region, in which the ball-speed may be defined.
   * 
   */
  public BallSpeedRegion() {

  }

  public void reinitRegion(final BallInfo bi,
      final double headAngle,
      final BallRegion posregion,
      final Vektor mySpeed) {

    this.headangle = headAngle;
    this.mySpeed = mySpeed;
    this.minDeltaR = posregion.minDistance * (Math
        .rint(bi.getDistChange() / (bi.getDistance() * .02)) - .5) * .02;
    this.maxDeltaR = posregion.maxDistance * (Math
        .rint(bi.getDistChange() / (bi.getDistance() * .02)) + .5) * .02;

    this.minDeltaPhi = (Math.rint(bi.getDirChange() / 0.1) - 0.05) * 0.1;
    this.maxDeltaPhi = (Math.rint(bi.getDirChange() / 0.1) + 0.05) * 0.1;

    this.seenDistance = bi.getDistance();

    this.ex = Math.sin(bi.getDirection() * BallSpeedRegion.PI_180);
    this.ey = Math.cos(bi.getDirection() * BallSpeedRegion.PI_180);
  }

  public Vektor makeRandomInsideRegion(Vektor point) {

    if (point == null) {
      point = new Vektor();
    }

    final double randDeltaPhi = this.minDeltaPhi + this.random.nextDouble() * (this.maxDeltaPhi - this.minDeltaPhi);
    final double randDeltaR = this.minDeltaR + this.random.nextDouble() * (this.maxDeltaR - this.minDeltaR);

    point.x = randDeltaR * this.ex + randDeltaPhi * BallSpeedRegion.PI_180 * this.seenDistance * this.ey;
    point.y = randDeltaR * this.ey - randDeltaPhi * BallSpeedRegion.PI_180 * this.seenDistance * this.ex;

    point.rotate(this.headangle);
    point.addToThis(this.mySpeed);
    return point;
  }

  public double insideRegion(final Vektor v) {

    this.helperVektor.x = v.x;
    this.helperVektor.y = v.y;
    this.helperVektor.subFromThis(this.mySpeed);
    this.helperVektor.rotate(-this.headangle);
    final double DeltaPhi = BallSpeedRegion._180_PI * (v.x * this.ey - v.y * this.ex) / this.seenDistance;
    final double DeltaR = v.x * this.ex + v.y * this.ey;

    if (this.minDeltaPhi <= DeltaPhi && DeltaPhi <= this.maxDeltaPhi && this.minDeltaR <= DeltaR && DeltaR <= this.maxDeltaR) {
      return 1.0;
    }
    else {
      return -1;
    }
  }

}
